/*
clsquare - closed loop simulation system
Copyright (c) 2004, Neuroinformatics Group, Prof. Dr. Martin Riedmiller,
University of Osnabrueck

Author: Roland Hafner
All rights reserved.

Redistribution and use in source and binary forms, with or without
modification, are permitted provided that the following conditions are
met:

   * Redistributions of source code must retain the above copyright
     notice, this list of conditions and the following disclaimer.
   * Redistributions in binary form must reproduce the above copyright
     notice, this list of conditions and the following disclaimer in
     the documentation and/or other materials provided with the
     distribution.
   * Neither the name of the <ORGANIZATION> nor the names of its
     contributors may be used to endorse or promote products derived
     from this software without specific prior written permission.

THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
"AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR
A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT
OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT
LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE,
DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY
THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH
DAMAGE. 
*/

#include "input.h"
#include "valueparser.h"
#include "global.h"
#include "str2val.h"
#include <stdio.h>
#include <cstdlib>
#include <cstring>
#include <sstream>
#include <fstream>

void Input::get_state_mode0(double* initial_state)
{
  int h = input_set.numSubsets();
  if (h<=0) EOUT("No subsets for init defined!");
  if (h>1) h=(int) (h * drand48()); // choose one subset randomly
  else (h-=1);
  for(int i=0; i<state_dim; i++) {
    double range  = input_set.subsets[h][i].max - input_set.subsets[h][i].min;
    double offset = input_set.subsets[h][i].min;
    initial_state[i] = offset;
    if(range== 0) continue;
    initial_state[i] += range * drand48();
  }
}

void Input::get_state_mode1(double* initial_state)
{
  if (stups.size()<=0) 
    {
      EOUT("No startup positions loaded from file!");
      return;
    }
  
  double r=0;
  int tmp_id;

  switch (order_of_presentation) {
  case (0) :
    tmp_id = act_stup_id;
    act_stup_id+=1;
    if (act_stup_id >= (int)stups.size()) act_stup_id=0;
    break;
  case (1) :
    r = drand48();
    tmp_id = (int) (r * stups.size());
    break;
  default:
    EOUT("No such order of presentation: (" <<  order_of_presentation << ")!");
    return;
  }
  
  for (int i=0; i<state_dim; i++) {
    initial_state[i]=stups[tmp_id][i];
  }
}

/*****************************************************************
 * Input: set new state
 *****************************************************************/

bool Input::get_initial_state(double* initial_state){
  
  episode_counter++;

  switch (input_mode) {
  case -1:
    break;
  case 0:
    get_state_mode0(initial_state);
    break;
  case 1:
    get_state_mode1(initial_state);
    break;
  default:
    EOUT("Unknown input_mode: (" << input_mode << ")!");
    return false;
  }

  for(int i=0;i<state_dim;i++){
    initial_state[i] = initial_state[i] + drand48() * distortion_vec[i];
  }

  
  return true;
}


bool Input::get_reference_input(double* reference_input, long cycle_ctr)
{
  for (int i=0; i<reference_input_dim; i++) {
    if ( ref_inp_fun[i] == 0)
      reference_input[i]=0.0;
    else
      reference_input[i] = ref_inp_fun[i]->get( cycle_ctr * delta_t );
  }
  return true;
}


bool Input::deinit(){
  for (int i=0; i<reference_input_dim; i++) if (ref_inp_fun[i]!=0) delete ref_inp_fun[i];
  return true;
}

bool Input::init(const int _state_dim, const int _action_dim, const int _reference_input_dim, double _delta_t, const char *fname)
{
  state_dim       = _state_dim;
  action_dim            = _action_dim;
  reference_input_dim   = _reference_input_dim;
  delta_t               = _delta_t;
  episode_counter       = 0;
  verbosity             = 0;
  
  //defaults
  input_mode            = 0; //< random from interval
  order_of_presentation = 0; //< for mode 1 only

  ref_inp_fun.resize(_reference_input_dim);
  for (int i=0; i<_reference_input_dim; i++) ref_inp_fun[i]=0;
  

  distortion_vec = new double[state_dim];
  for(int i= 0; i<state_dim; i++)
    distortion_vec[i] = 0;

  if(read_options(fname) == false)
    return false;
  
  if(verbosity>0){
    cout<<"Input states from file: "<<endl;
    for(int n=0; n<(int)stups.size();n++){
      for(int i=0;i<state_dim;i++)
	cout<<stups[n][i]<<"  ";
      cout<<endl;
    }
  }
  
  act_stup_id = 0;
  
  return true;
}

bool Input::read_options(const char * fname) {
  char paramstr[MAX_STR_LEN];
  
  if(fname == 0)
    return true;
  
  ValueParser vp(fname,"Input");

  vp.get("verbosity", verbosity);
  
  if(vp.get("input_mode",paramstr,MAX_STR_LEN)>=0){
    if(strcmp(paramstr,"from_file")== 0)
      input_mode = 1;
    else if(strcmp(paramstr,"random")== 0)
      input_mode = 0;
    else if(strcmp(paramstr,"none")== 0)
      input_mode = -1;
    else
      EOUT("Unknown input mode: (" << paramstr << ") using default (random)!");
  }
  
  if (input_mode == -1)
    return true;


  vp.get("distortion_vec",distortion_vec, state_dim);
  
  
  if (input_mode == 0) {
    if(vp.get("init",paramstr,MAX_STR_LEN)>=0) {
      input_set.parseStr(paramstr, state_dim);
    }
    else {
      EOUT("Missing parameter 'init' in cfg file section [INPUT]!)");
      return false;
    }
    
  }   
  else if (input_mode == 1) {
    if(vp.get("input_file",paramstr,MAX_STR_LEN)>=0) {
      if (!parse_input_file(paramstr)) return false;
    }
    else {
      EOUT("Missing Parameter (input_file) in cfg file section [INPUT]!");
      return false;
    }
    if(vp.get("order_of_presentation",paramstr,MAX_STR_LEN)>=0){
      if(strcmp(paramstr,"random")== 0)
	order_of_presentation = 1;
      else if(strcmp(paramstr,"serial")== 0)
	order_of_presentation = 0;
      else {
	EOUT("Unknown (order_of_presentation) value: " << paramstr << ") using default (serial)!");
	order_of_presentation = 0;
	// default: 0 (serial)
      }
    }
  }
  else {
    EOUT("Internal!! Unknown input parameter " << input_mode);
    return false;
  }
  
  if (reference_input_dim>0) {
    for (int i=0; i<reference_input_dim; i++) {
      std::stringstream pname;
      pname << "ref_inp_fun_" << i;
      if (vp.get(pname.str().c_str(),paramstr,MAX_STR_LEN)>=0) {
	ref_inp_fun[i] = new InterpolLinFun1D();
	if (!ref_inp_fun[i]->load(paramstr)) return false;
      }
    }
  }

  return true;
}



bool Input::parse_input_file(const char *fname)
{
  std::istream *fin= 0;
  fin = new std::ifstream(fname);
  
  if (!fin || !(*fin))
    {
      EOUT("Can't open file: " << fname);
      return false;
    }
  
  char  dum;
  double fdum;
  while (!(fin->eof()))
    { 
      // skipp white spaces
      (*fin) >> std::ws;
      
      fin->get(dum);
      if (!(dum == '#' || dum == '\n'))
	{
	  std::vector<double> stup;
	  fin->unget();
	  for (int i=0; i<state_dim; i++) {
	    if ((*fin) >> fdum)
	      stup.push_back(fdum);	      
	  }
	  if (((int) stup.size()) == state_dim)
	    stups.push_back(stup);
	  else {
	    EOUT("Error on parsing input file " << fname << "not enough data in line.");
	    continue;
	  }
	}
      
      // skip rest of line
      do { fin->get(dum); } 
      while( (dum != '\n') && (!fin->eof()) );
    }
  if (fin) {
    delete fin;
  }
  
  return true;
}
